#/usr/bin/python3
import serial
import time
import math
from enum import Enum
header = 0x55

class servo(object):
    class Command(Enum):#cmd's Value
        move_time_write = 1
        move_time_read =2
        move_time_wait_write=7
        move_time_wait_read=8
        move_start = 11
        move_stop =12
        id_write = 13
        id_read = 14
        angle_offset_adjust = 17
        angle_offset_write =18
        angle_offset_read = 19
        angle_limit_write =20
        angle_limit_read = 21
        vin_limit_write = 22
        vin_limit_read = 23
        temp_max_limit_write = 24
        temp_max_limit_read = 25
        temp_read = 26
        vin_read = 27
        pos_read = 28
        mode_write = 29
        mode_read = 30
        load_write = 31
        load_read = 32
    def __init__(self,port:str,id:int) -> None:#init serial
        self.ser =serial.Serial(port=port,baudrate=115200,rtscts=True)
        self.id = id
    def build_packet(self,cmd,params:bytearray):#build package
        packct = bytearray( )
        length = 1 + 1 + len(params) +1
        packct.append(header)
        packct.append(header)
        packct.append(self.id)
        packct.append(length)
        packct.append(cmd)

        packct.extend(params)

        checksum = self.id +length +cmd
        for param in params:
            checksum += param
        checksum = checksum % 256
        
        packct.append(~checksum & 0xFF)

        return packct
    def send(self,data:bytearray):
        self.ser.write(data)
    def move_time_write(self,angle,time):
        if not 0 <= angle <=240:
            raise ValueError()
        if not 0 <= time <= 30000:
            raise ValueError()
        
        angle_value = math.floor(angle/0.24)
        time_value = time

        packet = self.build_packet(
            self.Command.move_time_write.value,bytearray(
                [
                    angle_value & 0xFF,
                    angle_value >> 8,
                    time_value & 0xFF,
                    time_value >> 8,
                ]
            ))
        self.send(packet)
    
    def move_time_read(self):
        cmd = self.Command.move_time_read.value
        packet = self.build_packet(cmd,bytearray())
        self.send(packet)

        self.ser.read_until(expected = b'\xff' )
        if not self.ser.read() == header:
            raise ValueError()
        if not self.ser.read() == self.id:
            raise ValueError()
        length = self.ser.read()
        params = self.ser.read(length -1 -1)
        checksum = self.ser.read()
        checksum_expected = self.id + length + cmd

        for p in params:
            checksum_expected += p
        checksum_expected = checksum_expected % 256
        checksum_expected = ~checksum_expected & 0xFF

        if not checksum_expected == checksum:
            raise ValueError
        
        angle = (params[0] + (params[1] << 8)) * 0.24
        time = params[2] + (params[3] << 8)

        return angle,time


if __name__ == '__main__':
    Ser = servo("/dev/ttyAMA0",6)#/dev/ttyAMA0
    print(Ser.ser.name,Ser.ser.baudrate,Ser.ser.isOpen())
    if Ser.ser.isOpen() == False:
        Ser.ser.open()
    print(Ser.ser.name,Ser.ser.baudrate,Ser.ser.isOpen())
    Ser.move_time_write(240,10000)
    Ser.ser.close()
    print(Ser.ser.name,Ser.ser.baudrate,Ser.ser.isOpen())